/*
 * PairwiseLineMatching.cpp
 *
 *  Created on: 2011-8-19
 *      Author: lz
 */

#include "PairwiseLineMatching.hh"
#include <arlsmat.h>
#include <arlssym.h>
#include <vector>
#include <math.h>
#include <iostream>
#include <opencv2/line_descriptor/descriptor.hpp>

using namespace std;
using namespace cv;
using namespace cv::line_descriptor;


#define Inf 1e10 //Infinity
//the resolution scale of theta histogram, used when compute angle histogram of lines in the image
#define ResolutionScale 20 //10 degree
/*The following two thresholds are used to decide whether the estimated global rotation is acceptable.
 *Some image pairs don't have a stable global rotation angle, e.g. the image pair of the wide baseline
 *non planar scene. */
#define AcceptableAngleHistogramDifference 0.49
#define AcceptableLengthVectorDifference 0.4

/*The following four thresholds are used to decide whether a line in the left and a line in the right
 *are the possible matched line pair. If they are, then their differences should be smaller than these
 *thresholds.*/
#define LengthDifThreshold 4
#define AngleDifferenceThreshold 0.7854 //45degree
#define DescriptorDifThreshold 0.35		//0.35, or o.5 are good values for LBD
#define BinaryDescriptorDifThreshold 32		//0.35, or o.5 are good values for LBD
/*The following four threshold are used to decide whether the two possible matched line pair agree with each other.
 *They reflect the similarity of pairwise geometric information.
 */
#define RelativeAngleDifferenceThreshold 0.7854 //45degree
#define IntersectionRationDifThreshold 1
#define ProjectionRationDifThreshold 1

//this is used when get matching result from principal eigen vector
#define WeightOfMeanEigenVec 0.1

double getNormL2(double *arr, int size)
{
	double result = 0;
	for (int i = 0; i < size; i++)
	{
        result = result + arr[i] * arr[i];
	}
	return sqrt(result);
}
void arrayMultiRatio(double *arr, int size, double ratio)
{
	for (int i = 0; i < size; i++)
	{
		arr[i] = arr[i] * ratio;
	}
}
bool matSave(double **mat, int n, int m, const std::string &filename)
{
	std::ofstream fs(filename.c_str());
	if (!fs)
	{
		return false;
	}
	else
	{
		fs << (*mat);
		fs.close();
	};
	return true;
}

int DescriptorDistance(const Mat &a, const Mat &b)
{
    const int *pa = a.ptr<int32_t>();
    const int *pb = b.ptr<int32_t>();

    int dist=0;

    for(int i=0; i<8; i++, pa++, pb++)
    {
        unsigned  int v = *pa ^ *pb;
        v = v - ((v >> 1) & 0x55555555);
        v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
        dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
    }

    return dist;
}

void PairwiseLineMatching::LineMatching(ScaleLines &linesInLeft, ScaleLines &linesInRight, std::vector<unsigned int> &matchResult)
{
	//compute the global rotation angle of image pair;
	globalRotationAngle_ = GlobalRotationOfImagePair_(linesInLeft, linesInRight);
    BuildAdjacencyMatrix_(linesInLeft, linesInRight);
	MatchingResultFromPrincipalEigenvector_(linesInLeft, linesInRight, matchResult);
}

void PairwiseLineMatching::BinaryLineMatching(vector<KeyLine> &linesInLeft, vector<KeyLine> &linesInRight,
                                              Mat& left_ldesc,Mat& right_ldesc,std::vector<unsigned int> &matchResult)
{
    //compute the global rotation angle of image pair;
    globalRotationAngle_ = BinaryGlobalRotationOfImagePair_(linesInLeft, linesInRight);
    BinaryBuildAdjacencyMatrix_(linesInLeft, linesInRight,left_ldesc,right_ldesc);
    BinaryMatchingResultFromPrincipalEigenvector_(linesInLeft, linesInRight, left_ldesc,right_ldesc,matchResult);
}

double PairwiseLineMatching::GlobalRotationOfImagePair_(ScaleLines &linesInLeft, ScaleLines &linesInRight)
{
	double TwoPI = 2 * M_PI;
	double rotationAngle = TwoPI;

	//step 1: compute the angle histogram of lines in the left and right images
	const unsigned int dim = 360 / ResolutionScale; //number of the bins of histogram
	unsigned int index;								//index in the histogram
	double direction;
	double scalar = 180 / (ResolutionScale * 3.1415927); //used when compute the index
	double angleShift = (ResolutionScale * M_PI) / 360;  //make sure zero is the middle of the interval

	std::array<double, dim> angleHistLeft;
	std::array<double, dim> angleHistRight;
	std::array<double, dim> lengthLeft; //lengthLeft[i] store the total line length of all the lines in the ith angle bin.
	std::array<double, dim> lengthRight;
	angleHistLeft.fill(0);
	angleHistRight.fill(0);
	lengthLeft.fill(0);
	lengthRight.fill(0);

	for (unsigned int linenum = 0; linenum < linesInLeft.size(); linenum++)
	{
		direction = linesInLeft[linenum][0].direction + M_PI + angleShift;
		direction = direction < TwoPI ? direction : (direction - TwoPI);
		index = floor(direction * scalar);
		angleHistLeft[index]++;
		lengthLeft[index] += linesInLeft[linenum][0].lineLength;
	}
	for (unsigned int linenum = 0; linenum < linesInRight.size(); linenum++)
	{
		direction = linesInRight[linenum][0].direction + M_PI + angleShift;
		direction = direction < TwoPI ? direction : (direction - TwoPI);
		index = floor(direction * scalar);
		angleHistRight[index]++;
		lengthRight[index] += linesInRight[linenum][0].lineLength;
	}
	arrayMultiRatio(angleHistLeft.data(), angleHistLeft.size(), (1 / getNormL2(angleHistLeft.data(), angleHistLeft.size())));
	arrayMultiRatio(angleHistRight.data(), angleHistRight.size(), (1 / getNormL2(angleHistRight.data(), angleHistRight.size())));
	arrayMultiRatio(lengthLeft.data(), lengthLeft.size(), (1 / getNormL2(lengthLeft.data(), lengthLeft.size())));
	arrayMultiRatio(lengthRight.data(), lengthRight.size(), (1 / getNormL2(lengthRight.data(), lengthRight.size())));

	//  angleHistLeft.Save("histLeft.txt");
	//  angleHistRight.Save("histRight.txt");

	//step 2: find shift to decide the approximate global rotation
	std::array<double, dim> difVec; //the difference vector between left histogram and shifted right histogram
	double minDif = 10;				//the minimal angle histogram difference
	double secondMinDif = 10;		//the second minimal histogram difference
	unsigned int minShift;			//the shift of right angle histogram when minimal difference achieved
	unsigned int secondMinShift;	//the shift of right angle histogram when second minimal difference achieved

	std::array<double, dim> lengthDifVec; //the length difference vector between left and right
	double minLenDif = 10;				  //the minimal length difference
	double secondMinLenDif = 10;		  //the second minimal length difference
	unsigned int minLenShift;			  //the shift of right length vector when minimal length difference achieved
	unsigned int secondMinLenShift;		  //the shift of right length vector when the second minimal length difference achieved

	double normOfVec;
	for (unsigned int shift = 0; shift < dim; shift++)
	{
		for (unsigned int j = 0; j < dim; j++)
		{
			index = j + shift;
			index = index < dim ? index : (index - dim);
			difVec[j] = angleHistLeft[j] - angleHistRight[index];
			lengthDifVec[j] = lengthLeft[j] - lengthRight[index];
		}
		//find the minShift and secondMinShift for angle histogram
		normOfVec = getNormL2(difVec.data(), difVec.size());
		if (normOfVec < secondMinDif)
		{
			if (normOfVec < minDif)
			{
				secondMinDif = minDif;
				secondMinShift = minShift;
				minDif = normOfVec;
				minShift = shift;
			}
			else
			{
				secondMinDif = normOfVec;
				secondMinShift = shift;
			}
		}
		//find the minLenShift and secondMinLenShift of length vector
		normOfVec = getNormL2(lengthDifVec.data(), lengthDifVec.size());
		if (normOfVec < secondMinLenDif)
		{
			if (normOfVec < minLenDif)
			{
				secondMinLenDif = minLenDif;
				secondMinLenShift = minLenShift;
				minLenDif = normOfVec;
				minLenShift = shift;
			}
			else
			{
				secondMinLenDif = normOfVec;
				secondMinLenShift = shift;
			}
		}
	}

	//first check whether there exist an approximate global rotation angle between image pair
	if (minDif < AcceptableAngleHistogramDifference && minLenDif < AcceptableLengthVectorDifference)
	{
		rotationAngle = minShift * ResolutionScale;
        if (rotationAngle > 90 && 360 - rotationAngle > 90)
		{
			//In most case we believe the rotation angle between two image pairs should belong to [-Pi/2, Pi/2]
			rotationAngle = rotationAngle - 180;
		}
		rotationAngle = rotationAngle * M_PI / 180;
	}

	cout << "minimal histgram distance = " << minDif << ", Approximate global rotation angle = " << rotationAngle << endl;
	return rotationAngle;
}


double PairwiseLineMatching::BinaryGlobalRotationOfImagePair_(vector<KeyLine> &linesInLeft, vector<KeyLine> &linesInRight)
{
    double TwoPI = 2 * M_PI;
    double rotationAngle = TwoPI;

    //step 1: compute the angle histogram of lines in the left and right images
    const unsigned int dim = 360 / ResolutionScale; //number of the bins of histogram
    unsigned int index;								//index in the histogram
    double direction;
    double scalar = 180 / (ResolutionScale * 3.1415927); //used when compute the index
    double angleShift = (ResolutionScale * M_PI) / 360;  //make sure zero is the middle of the interval

    std::array<double, dim> angleHistLeft;
    std::array<double, dim> angleHistRight;
    std::array<double, dim> lengthLeft; //lengthLeft[i] store the total line length of all the lines in the ith angle bin.
    std::array<double, dim> lengthRight;
    angleHistLeft.fill(0);
    angleHistRight.fill(0);
    lengthLeft.fill(0);
    lengthRight.fill(0);

    for (unsigned int linenum = 0; linenum < linesInLeft.size(); linenum++)
    {
        direction = linesInLeft[linenum].angle + M_PI + angleShift;
        direction = direction < TwoPI ? direction : (direction - TwoPI);
        index = floor(direction * scalar);
        angleHistLeft[index]++;
        lengthLeft[index] += linesInLeft[linenum].lineLength;
    }

    for (unsigned int linenum = 0; linenum < linesInRight.size(); linenum++)
    {
        direction = linesInRight[linenum].angle + M_PI + angleShift;
        direction = direction < TwoPI ? direction : (direction - TwoPI);
        index = floor(direction * scalar);
        angleHistRight[index]++;
        lengthRight[index] += linesInRight[linenum].lineLength;
    }
    arrayMultiRatio(angleHistLeft.data(), angleHistLeft.size(), (1 / getNormL2(angleHistLeft.data(), angleHistLeft.size())));
    arrayMultiRatio(angleHistRight.data(), angleHistRight.size(), (1 / getNormL2(angleHistRight.data(), angleHistRight.size())));
    arrayMultiRatio(lengthLeft.data(), lengthLeft.size(), (1 / getNormL2(lengthLeft.data(), lengthLeft.size())));
    arrayMultiRatio(lengthRight.data(), lengthRight.size(), (1 / getNormL2(lengthRight.data(), lengthRight.size())));

    //  angleHistLeft.Save("histLeft.txt");
    //  angleHistRight.Save("histRight.txt");

    //step 2: find shift to decide the approximate global rotation
    std::array<double, dim> difVec; //the difference vector between left histogram and shifted right histogram
    double minDif = 10;				//the minimal angle histogram difference
    double secondMinDif = 10;		//the second minimal histogram difference
    unsigned int minShift;			//the shift of right angle histogram when minimal difference achieved
    unsigned int secondMinShift;	//the shift of right angle histogram when second minimal difference achieved

    std::array<double, dim> lengthDifVec; //the length difference vector between left and right
    double minLenDif = 10;				  //the minimal length difference
    double secondMinLenDif = 10;		  //the second minimal length difference
    unsigned int minLenShift;			  //the shift of right length vector when minimal length difference achieved
    unsigned int secondMinLenShift;		  //the shift of right length vector when the second minimal length difference achieved

    double normOfVec;
    for (unsigned int shift = 0; shift < dim; shift++)
    {
        for (unsigned int j = 0; j < dim; j++)
        {
            index = j + shift;
            index = index < dim ? index : (index - dim);
            difVec[j] = angleHistLeft[j] - angleHistRight[index];
            lengthDifVec[j] = lengthLeft[j] - lengthRight[index];
        }
        //find the minShift and secondMinShift for angle histogram
        normOfVec = getNormL2(difVec.data(), difVec.size());
        if (normOfVec < secondMinDif)
        {
            if (normOfVec < minDif)
            {
                secondMinDif = minDif;
                secondMinShift = minShift;
                minDif = normOfVec;
                minShift = shift;
            }
            else
            {
                secondMinDif = normOfVec;
                secondMinShift = shift;
            }
        }
        //find the minLenShift and secondMinLenShift of length vector
        normOfVec = getNormL2(lengthDifVec.data(), lengthDifVec.size());
        if (normOfVec < secondMinLenDif)
        {
            if (normOfVec < minLenDif)
            {
                secondMinLenDif = minLenDif;
                secondMinLenShift = minLenShift;
                minLenDif = normOfVec;
                minLenShift = shift;
            }
            else
            {
                secondMinLenDif = normOfVec;
                secondMinLenShift = shift;
            }
        }
    }

    //first check whether there exist an approximate global rotation angle between image pair
    if (minDif < AcceptableAngleHistogramDifference && minLenDif < AcceptableLengthVectorDifference)
    {
        rotationAngle = minShift * ResolutionScale;
        if (rotationAngle > 90 && 360 - rotationAngle > 90)
        {
            //In most case we believe the rotation angle between two image pairs should belong to [-Pi/2, Pi/2]
            rotationAngle = rotationAngle - 180;
        }
        rotationAngle = rotationAngle * M_PI / 180;
    }

    cout << "minimal histgram distance = " << minDif << ", Approximate global rotation angle = " << rotationAngle << endl;
    return rotationAngle;
}



void PairwiseLineMatching::BuildAdjacencyMatrix_(ScaleLines &linesInLeft, ScaleLines &linesInRight)
{
	double TwoPI = 2 * M_PI;
	const unsigned int numLineLeft = linesInLeft.size();
	const unsigned int numLineRight = linesInRight.size();
	/*first step, find nodes which are possible correspondent lines in the left and right images according to
	 *their direction, gray value  and gradient magnitude.
     */
	nodesList_.clear();
	double angleDif;
	double lengthDif;

	unsigned int dimOfDes = linesInLeft[0][0].descriptor.size();
	double desDisMat[numLineLeft][numLineRight]; //store the descriptor distance of lines in left and right images.

	std::vector<float> desLeft;
	std::vector<float> desRight;

	//first compute descriptor distances

	float *desL, *desR, *desMax, *desOld;

	float minDis, dis, temp;
	for (int idL = 0; idL < numLineLeft; idL++)
	{
		short sameLineSize = linesInLeft[idL].size();
		for (int idR = 0; idR < numLineRight; idR++)
		{
			minDis = 100;
			short sameLineSizeR = linesInRight[idR].size();
			for (short lineIDInSameLines = 0; lineIDInSameLines < sameLineSize; lineIDInSameLines++)
			{
				desOld = linesInLeft[idL][lineIDInSameLines].descriptor.data();
				for (short lineIDInSameLinesR = 0; lineIDInSameLinesR < sameLineSizeR; lineIDInSameLinesR++)
				{
					desL = desOld;
					desR = linesInRight[idR][lineIDInSameLinesR].descriptor.data();
					desMax = desR + dimOfDes;
					dis = 0;
					while (desR < desMax)
					{
                        temp = *(desL++) - *(desR++); //discriptor minus save to temp
						dis += temp * temp;
					}
					dis = sqrt(dis);
					if (dis < minDis)
					{
						minDis = dis;
					}
				}
			} //end for(short lineIDInSameLines = 0; lineIDInSameLines<sameLineSize; lineIDInSameLines++)
			desDisMat[idL][idR] = minDis;
		} //end for(int idR=0; idR<rightSize; idR++)
	}	 // end for(int idL=0; idL<leftSize; idL++)


	for (unsigned int i = 0; i < numLineLeft; i++)
	{
		for (unsigned int j = 0; j < numLineRight; j++)
		{
			if (desDisMat[i][j] > DescriptorDifThreshold)
			{
				continue; //the descriptor difference is too large;
			}

			if (globalRotationAngle_ < TwoPI)
			{ //there exist a global rotation angle between two image
				lengthDif = fabs(linesInLeft[i][0].lineLength - linesInRight[j][0].lineLength) / MIN(linesInLeft[i][0].lineLength, linesInRight[j][0].lineLength);
                //cout<<"lengthDif:"<<lengthDif<<endl;
                if (lengthDif > LengthDifThreshold)
				{
					continue; //the length difference is too large;
				}
				angleDif = fabs(linesInLeft[i][0].direction + globalRotationAngle_ - linesInRight[j][0].direction);
				//cout<<"angleDif:"<<angleDif<<endl;
                if (fabs(TwoPI - angleDif) > AngleDifferenceThreshold && angleDif > AngleDifferenceThreshold)
				{
					continue; //the angle difference is too large;
				}

				pNode node; //line i in left image and line j in right image pass the test, (i,j) is a possible matched line pair.
				node.leftLineID = i;
				node.rightLineID = j;
				nodesList_.push_back(node);
			}
			else
			{ //there doesn't exist a global rotation angle between two image, so the angle difference test is canceled.
				lengthDif = fabs(linesInLeft[i][0].lineLength - linesInRight[j][0].lineLength) / MIN(linesInLeft[i][0].lineLength, linesInRight[j][0].lineLength);
				if (lengthDif > LengthDifThreshold)
				{
					continue; //the length difference is too large;
				}
				pNode node; //line i in left image and line j in right image pass the test, (i,j) is a possible matched line pair.
				node.leftLineID = i;
				node.rightLineID = j;
				nodesList_.push_back(node);
			}
		} //end inner loop
	}
	cout << "the number of possible matched line pair = " << nodesList_.size() << endl;
	//	desDisMat.Save("DescriptorDis.txt");

	/*Second step, build the adjacency matrix which reflect the geometric constraints between nodes.
	 *The matrix is stored in the Compressed Sparse Column(CSC) format.
     */
	unsigned int dim = nodesList_.size(); // Dimension of the problem.

	//std::array<double, dim_temp> adjacenceVec;
	std::vector<double> adjacenceVec(dim * (dim + 1) / 2, 0);

	int nnz = 0; // Number of nonzero elements in adjacenceMat.
	/*adjacenceVec only store the lower part of the adjacency matrix which is a symmetric matrix.
	 *                    | 0  1  0  2  0 |
	 *                    | 1  0  3  0  1 |
	 *eg:  adjMatrix =    | 0  3  0  2  0 |
	 *                    | 2  0  2  0  3 |
	 *                    | 0  1  0  3  0 |
	 *     adjacenceVec = [0,1,0,2,0,0,3,0,1,0,2,0,0,3,0]
	 */
	//	Matrix<double> testMat(dim,dim);
	//	testMat.SetZero();

	/*In order to save computational time, the following variables are used to store
	 *the pairwise geometric information which has been computed and will be reused many times
	 *latter. The reduction of computational time is at the expenses of memory consumption.
	 */
    unsigned int bComputedLeft[numLineLeft][numLineLeft]; //flag to show whether the ith pair of left image has already been computed.
    memset(bComputedLeft, 0, numLineLeft * numLineLeft * sizeof(unsigned int));
    double intersecRatioLeft[numLineLeft][numLineLeft]; //the ratio of intersection point and the line in the left pair
    double projRatioLeft[numLineLeft][numLineLeft];		//the point to line distance divided by the projected length of line in the left pair.

    unsigned int bComputedRight[numLineRight][numLineRight]; //flag to show whether the ith pair of right image has already been computed.
    memset(bComputedRight, 0, numLineRight * numLineRight * sizeof(unsigned int));
    double intersecRatioRight[numLineRight][numLineRight]; //the ratio of intersection point and the line in the right pair
    double projRatioRight[numLineRight][numLineRight];	 //the point to line distance divided by the projected length of line in the right pair.

    unsigned int idLeft1, idLeft2;						//the id of lines in the left pair
	unsigned int idRight1, idRight2;					//the id of lines in the right pair
	double relativeAngleLeft, relativeAngleRight;		//the relative angle of each line pair
	double gradientMagRatioLeft, gradientMagRatioRight; //the ratio of gradient magnitude of lines in each pair

	double iRatio1L, iRatio1R, iRatio2L, iRatio2R;
	double pRatio1L, pRatio1R, pRatio2L, pRatio2R;

	double relativeAngleDif, gradientMagRatioDif, iRatioDif, pRatioDif;

	double interSectionPointX, interSectionPointY;
	double a1, a2, b1, b2, c1, c2; //line1: a1 x + b1 y + c1 =0; line2: a2 x + b2 y + c2=0
	double a1b2_a2b1;			   //a1b2-a2b1
	double length1, length2, len;
	double disX, disY;
	double disS, disE;
	double similarity;


	for (unsigned int j = 0; j < dim; j++)
	{ //column
		idLeft1 = nodesList_[j].leftLineID;
		idRight1 = nodesList_[j].rightLineID;
		for (unsigned int i = j + 1; i < dim; i++)
        { //row
			idLeft2 = nodesList_[i].leftLineID;
			idRight2 = nodesList_[i].rightLineID;
			if ((idLeft1 == idLeft2) || (idRight1 == idRight2))
			{
				continue; //not satisfy the one to one match condition
			}
			//first compute the relative angle between left pair and right pair.
			relativeAngleLeft = linesInLeft[idLeft1][0].direction - linesInLeft[idLeft2][0].direction;
			relativeAngleLeft = (relativeAngleLeft < M_PI) ? relativeAngleLeft : (relativeAngleLeft - TwoPI);
			relativeAngleLeft = (relativeAngleLeft > (-M_PI)) ? relativeAngleLeft : (relativeAngleLeft + TwoPI);
			relativeAngleRight = linesInRight[idRight1][0].direction - linesInRight[idRight2][0].direction;
			relativeAngleRight = (relativeAngleRight < M_PI) ? relativeAngleRight : (relativeAngleRight - TwoPI);
			relativeAngleRight = (relativeAngleRight > (-M_PI)) ? relativeAngleRight : (relativeAngleRight + TwoPI);
			relativeAngleDif = fabs(relativeAngleLeft - relativeAngleRight);
			if ((TwoPI - relativeAngleDif) > RelativeAngleDifferenceThreshold && relativeAngleDif > RelativeAngleDifferenceThreshold)
			{
				continue; //the relative angle difference is too large;
			}
			else if ((TwoPI - relativeAngleDif) < RelativeAngleDifferenceThreshold)
			{
				relativeAngleDif = TwoPI - relativeAngleDif;
			}

			//at last, check the intersect point ratio and point to line distance ratio
			//check whether the geometric information of pairs (idLeft1,idLeft2) and (idRight1,idRight2) have already been computed.
			if (!bComputedLeft[idLeft1][idLeft2])
			{ //have not been computed yet

				a1 = linesInLeft[idLeft1][0].endPointY - linesInLeft[idLeft1][0].startPointY;					//disY
				b1 = linesInLeft[idLeft1][0].startPointX - linesInLeft[idLeft1][0].endPointX;					//-disX
				c1 = (0 - b1 * linesInLeft[idLeft1][0].startPointY) - a1 * linesInLeft[idLeft1][0].startPointX; //disX*sy - disY*sx
				length1 = linesInLeft[idLeft1][0].lineLength;

				a2 = linesInLeft[idLeft2][0].endPointY - linesInLeft[idLeft2][0].startPointY;					//disY
				b2 = linesInLeft[idLeft2][0].startPointX - linesInLeft[idLeft2][0].endPointX;					//-disX
				c2 = (0 - b2 * linesInLeft[idLeft2][0].startPointY) - a2 * linesInLeft[idLeft2][0].startPointX; //disX*sy - disY*sx
				length2 = linesInLeft[idLeft2][0].lineLength;

				a1b2_a2b1 = a1 * b2 - a2 * b1;
				if (fabs(a1b2_a2b1) < 0.001)
				{ //two lines are almost parallel
					iRatio1L = Inf;
					iRatio2L = Inf;
				}
				else
				{
					interSectionPointX = (c2 * b1 - c1 * b2) / a1b2_a2b1;
					interSectionPointY = (c1 * a2 - c2 * a1) / a1b2_a2b1;
					//r1 = (s1I*s1e1)/(|s1e1|*|s1e1|)
					disX = interSectionPointX - linesInLeft[idLeft1][0].startPointX;
					disY = interSectionPointY - linesInLeft[idLeft1][0].startPointY;
					len = disY * a1 - disX * b1;    //交点到起点的距离与起点到终点的距离之比
					iRatio1L = len / (length1 * length1);
					//r2 = (s2I*s2e2)/(|s2e2|*|s2e2|)
					disX = interSectionPointX - linesInLeft[idLeft2][0].startPointX;
					disY = interSectionPointY - linesInLeft[idLeft2][0].startPointY;
					len = disY * a2 - disX * b2;
					iRatio2L = len / (length2 * length2);
				}
				intersecRatioLeft[idLeft1][idLeft2] = iRatio1L;
				intersecRatioLeft[idLeft2][idLeft1] = iRatio2L; //line order changed

				/*project the end points of line1 onto line2 and compute their distances to line2;
				 */
				disS = fabs(a2 * linesInLeft[idLeft1][0].startPointX + b2 * linesInLeft[idLeft1][0].startPointY + c2) / length2;
                //l1线段的起点到l2线段的距离与l2线段的长度之比
				disE = fabs(a2 * linesInLeft[idLeft1][0].endPointX + b2 * linesInLeft[idLeft1][0].endPointY + c2) / length2;
                //l1线段的终点到l2线段的距离与l2线段的长度之比
                pRatio1L = (disS + disE) / length1;
				projRatioLeft[idLeft1][idLeft2] = pRatio1L;

				/*project the end points of line2 onto line1 and compute their distances to line1;
				 */
				disS = fabs(a1 * linesInLeft[idLeft2][0].startPointX + b1 * linesInLeft[idLeft2][0].startPointY + c1) / length1;
				disE = fabs(a1 * linesInLeft[idLeft2][0].endPointX + b1 * linesInLeft[idLeft2][0].endPointY + c1) / length1;
				pRatio2L = (disS + disE) / length2;
				projRatioLeft[idLeft2][idLeft1] = pRatio2L;

				//mark them as computed
				bComputedLeft[idLeft1][idLeft2] = true;
				bComputedLeft[idLeft2][idLeft1] = true;
			}
			else
			{ //read these information from matrix;
				iRatio1L = intersecRatioLeft[idLeft1][idLeft2];
				iRatio2L = intersecRatioLeft[idLeft2][idLeft1];
				pRatio1L = projRatioLeft[idLeft1][idLeft2];
				pRatio2L = projRatioLeft[idLeft2][idLeft1];
			}
            if (!bComputedRight[idRight1][idRight2])
            {
                //have not been computed yet
                a1 = linesInRight[idRight1][0].endPointY - linesInRight[idRight1][0].startPointY;					//disY
                b1 = linesInRight[idRight1][0].startPointX - linesInRight[idRight1][0].endPointX;					//-disX
                c1 = (0 - b1 * linesInRight[idRight1][0].startPointY) - a1 * linesInRight[idRight1][0].startPointX; //disX*sy - disY*sx
                length1 = linesInRight[idRight1][0].lineLength;

                a2 = linesInRight[idRight2][0].endPointY - linesInRight[idRight2][0].startPointY;					//disY
                b2 = linesInRight[idRight2][0].startPointX - linesInRight[idRight2][0].endPointX;					//-disX
                c2 = (0 - b2 * linesInRight[idRight2][0].startPointY) - a2 * linesInRight[idRight2][0].startPointX; //disX*sy - disY*sx
                length2 = linesInRight[idRight2][0].lineLength;

                a1b2_a2b1 = a1 * b2 - a2 * b1;
                if (fabs(a1b2_a2b1) < 0.001)
                { //two lines are almost parallel
                    iRatio1R = Inf;
                    iRatio2R = Inf;
                }
                else
                {
                    interSectionPointX = (c2 * b1 - c1 * b2) / a1b2_a2b1;
                    interSectionPointY = (c1 * a2 - c2 * a1) / a1b2_a2b1;
                    //r1 = (s1I*s1e1)/(|s1e1|*|s1e1|)
                    disX = interSectionPointX - linesInRight[idRight1][0].startPointX;
                    disY = interSectionPointY - linesInRight[idRight1][0].startPointY;
                    len = disY * a1 - disX * b1; //because b1=-disX
                    iRatio1R = len / (length1 * length1);
                    //r2 = (s2I*s2e2)/(|s2e2|*|s2e2|)
                    disX = interSectionPointX - linesInRight[idRight2][0].startPointX;
                    disY = interSectionPointY - linesInRight[idRight2][0].startPointY;
                    len = disY * a2 - disX * b2; //because b2=-disX
                    iRatio2R = len / (length2 * length2);
                }
                intersecRatioRight[idRight1][idRight2] = iRatio1R;
                intersecRatioRight[idRight2][idRight1] = iRatio2R; //line order changed
                /*project the end points of line1 onto line2 and compute their distances to line2;
                 */
                disS = fabs(a2 * linesInRight[idRight1][0].startPointX + b2 * linesInRight[idRight1][0].startPointY + c2) / length2;
                disE = fabs(a2 * linesInRight[idRight1][0].endPointX + b2 * linesInRight[idRight1][0].endPointY + c2) / length2;
                pRatio1R = (disS + disE) / length1;
                projRatioRight[idRight1][idRight2] = pRatio1R;

                /*project the end points of line2 onto line1 and compute their distances to line1;
                 */
                disS = fabs(a1 * linesInRight[idRight2][0].startPointX + b1 * linesInRight[idRight2][0].startPointY + c1) / length1;
                disE = fabs(a1 * linesInRight[idRight2][0].endPointX + b1 * linesInRight[idRight2][0].endPointY + c1) / length1;
                pRatio2R = (disS + disE) / length2;
                projRatioRight[idRight2][idRight1] = pRatio2R;

                //mark them as computed
                bComputedRight[idRight1][idRight2] = true;
                bComputedRight[idRight2][idRight1] = true;
            }
            else
            { //read these information from matrix;
                iRatio1R = intersecRatioRight[idRight1][idRight2];
                iRatio2R = intersecRatioRight[idRight2][idRight1];
                pRatio1R = projRatioRight[idRight1][idRight2];
                pRatio2R = projRatioRight[idRight2][idRight1];
            }

            pRatioDif = MIN(fabs(pRatio1L - pRatio1R), fabs(pRatio2L - pRatio2R));

			if (pRatioDif > ProjectionRationDifThreshold)
			{
				continue; //the projection length ratio difference is too large;
			}
			if ((iRatio1L == Inf) || (iRatio2L == Inf) || (iRatio1R == Inf) || (iRatio2R == Inf))
            {
				//don't consider the intersection length ratio
				similarity = 4 - desDisMat[idLeft1][idRight1] / DescriptorDifThreshold - desDisMat[idLeft2][idRight2] / DescriptorDifThreshold - pRatioDif / ProjectionRationDifThreshold - relativeAngleDif / RelativeAngleDifferenceThreshold;
                adjacenceVec[(2 * dim - j - 1) * j / 2 + i] = similarity;
				nnz++;
				//				testMat[i][j] = similarity;
				//				testMat[j][i] = similarity;
			}
			else
            {
				iRatioDif = MIN(fabs(iRatio1L - iRatio1R), fabs(iRatio2L - iRatio2R));
				if (iRatioDif > IntersectionRationDifThreshold)
				{
					continue; //the intersection length ratio difference is too large;
				}
				//now compute the similarity score between two line pairs.
				similarity = 5 - desDisMat[idLeft1][idRight1] / DescriptorDifThreshold - desDisMat[idLeft2][idRight2] / DescriptorDifThreshold - iRatioDif / IntersectionRationDifThreshold - pRatioDif / ProjectionRationDifThreshold - relativeAngleDif / RelativeAngleDifferenceThreshold;
				adjacenceVec[(2 * dim - j - 1) * j / 2 + i] = similarity;
				nnz++;
				//				testMat[i][j] = similarity;
				//				testMat[j][i] = similarity;
			}
		}
    }
	// pointer to an array that stores the nonzero elements of Adjacency matrix.
	double *adjacenceMat = new double[nnz];
	// pointer to an array that stores the row indices of the non-zeros in adjacenceMat.
	int *irow = new int[nnz];
	// pointer to an array of pointers to the beginning of each column of adjacenceMat.
	int *pcol = new int[dim + 1];
	int idOfNNZ = 0; //the order of none zero element
	pcol[0] = 0;
	unsigned int tempValue;
	for (unsigned int j = 0; j < dim; j++)
	{ //column
		for (unsigned int i = j; i < dim; i++)
		{ //row
			tempValue = (2 * dim - j - 1) * j / 2 + i;
			if (adjacenceVec[tempValue] != 0)
			{
				adjacenceMat[idOfNNZ] = adjacenceVec[tempValue];
				irow[idOfNNZ] = i;
				idOfNNZ++;
			}
		}
		pcol[j + 1] = idOfNNZ;
    }
	/*Third step, solve the principal eigenvector of the adjacency matrix using Arpack lib.
	 */
	ARluSymMatrix<double> arMatrix(dim, nnz, adjacenceMat, irow, pcol);
	ARluSymStdEig<double> dprob(2, arMatrix, "LM"); // Defining what we need: the first eigenvector of arMatrix with largest magnitude.
	// Finding eigenvalues and eigenvectors.
	dprob.FindEigenvectors();
    cout << "Number of 'converged' eigenvalues  : " << dprob.ConvergedEigenvalues() << endl;

	eigenMap_.clear();

	double meanEigenVec = 0;
	if (dprob.EigenvectorsFound())
	{
		double value;
		for (unsigned int j = 0; j < dim; j++)
		{
			value = fabs(dprob.Eigenvector(1, j));
			meanEigenVec += value;
			eigenMap_.insert(std::make_pair(value, j));
		}
	}
	minOfEigenVec_ = WeightOfMeanEigenVec * meanEigenVec / dim;
	delete[] adjacenceMat;
	delete[] irow;
	delete[] pcol;
}




void PairwiseLineMatching::BinaryBuildAdjacencyMatrix_(vector<KeyLine> &linesInLeft, vector<KeyLine> &linesInRight,Mat& left_ldesc,Mat& right_ldesc)
{
    double TwoPI = 2 * M_PI;
    const unsigned int numLineLeft = linesInLeft.size();
    const unsigned int numLineRight = linesInRight.size();
    /*first step, find nodes which are possible correspondent lines in the left and right images according to
     *their direction, gray value  and gradient magnitude.
     */
    nodesList_.clear();
    double angleDif;
    double lengthDif;

    int desDisMat[numLineLeft][numLineRight]; //store the descriptor distance of lines in left and right images.

    //first compute descriptor distances
    Mat desL, desR, desMax, desOld;
    int minDis, dis, temp;
    for (int idL = 0; idL < numLineLeft; idL++)
    {
        short sameLineSize = 1;
        for (int idR = 0; idR < numLineRight; idR++)
        {
            minDis = 256;
            short sameLineSizeR = 1;
            for (short lineIDInSameLines = 0; lineIDInSameLines < sameLineSize; lineIDInSameLines++)
            {
                desL = left_ldesc.row(idL);
                for (short lineIDInSameLinesR = 0; lineIDInSameLinesR < sameLineSizeR; lineIDInSameLinesR++)
                {
                    desR = right_ldesc.row(idR);
                    dis = DescriptorDistance(desL, desR);
                    if (dis < minDis)
                    {
                        minDis = dis;
                    }
                }
            } //end for(short lineIDInSameLines = 0; lineIDInSameLines<sameLineSize; lineIDInSameLines++)
            desDisMat[idL][idR] = minDis;
//            cout<<"mindis:"<<minDis<<endl;
        } //end for(int idR=0; idR<rightSize; idR++)
    }	 // end for(int idL=0; idL<leftSize; idL++)


    for (unsigned int i = 0; i < numLineLeft; i++)
    {
        for (unsigned int j = 0; j < numLineRight; j++)
        {
            if (desDisMat[i][j] > BinaryDescriptorDifThreshold)
            {
                continue; //the descriptor difference is too large;
            }

            if (globalRotationAngle_ < TwoPI)
            { //there exist a global rotation angle between two image
                lengthDif = fabs(linesInLeft[i].lineLength - linesInRight[j].lineLength) / MIN(linesInLeft[i].lineLength, linesInRight[j].lineLength);
                //cout<<"lengthDif:"<<lengthDif<<endl;
                if (lengthDif > LengthDifThreshold)
                {
                    continue; //the length difference is too large;
                }
                angleDif = fabs(linesInLeft[i].angle + globalRotationAngle_ - linesInRight[j].angle);
                //cout<<"angleDif:"<<angleDif<<endl;
                if (fabs(TwoPI - angleDif) > AngleDifferenceThreshold && angleDif > AngleDifferenceThreshold)
                {
                    continue; //the angle difference is too large;
                }

                pNode node; //line i in left image and line j in right image pass the test, (i,j) is a possible matched line pair.
                node.leftLineID = i;
                node.rightLineID = j;
                //cout<<"i:j="<<i<<":"<<j<<endl;
                nodesList_.push_back(node);
            }
            else
            { //there doesn't exist a global rotation angle between two image, so the angle difference test is canceled.
                lengthDif = fabs(linesInLeft[i].lineLength - linesInRight[j].lineLength) / MIN(linesInLeft[i].lineLength, linesInRight[j].lineLength);
                if (lengthDif > LengthDifThreshold)
                {
                    continue; //the length difference is too large;
                }
                pNode node; //line i in left image and line j in right image pass the test, (i,j) is a possible matched line pair.
                node.leftLineID = i;
                node.rightLineID = j;
                nodesList_.push_back(node);
            }
        } //end inner loop
    }
    cout << "the number of possible matched line pair = " << nodesList_.size() << endl;
    //	desDisMat.Save("DescriptorDis.txt");

    /*Second step, build the adjacency matrix which reflect the geometric constraints between nodes.
     *The matrix is stored in the Compressed Sparse Column(CSC) format.
     */
    unsigned int dim = nodesList_.size(); // Dimension of the problem.

    //std::array<double, dim_temp> adjacenceVec;
    std::vector<double> adjacenceVec(dim * (dim + 1) / 2, 0);

    int nnz = 0; // Number of nonzero elements in adjacenceMat.
    /*adjacenceVec only store the lower part of the adjacency matrix which is a symmetric matrix.
     *                    | 0  1  0  2  0 |
     *                    | 1  0  3  0  1 |
     *eg:  adjMatrix =    | 0  3  0  2  0 |
     *                    | 2  0  2  0  3 |
     *                    | 0  1  0  3  0 |
     *     adjacenceVec = [0,1,0,2,0,0,3,0,1,0,2,0,0,3,0]
     */
    //	Matrix<double> testMat(dim,dim);
    //	testMat.SetZero();

    /*In order to save computational time, the following variables are used to store
     *the pairwise geometric information which has been computed and will be reused many times
     *latter. The reduction of computational time is at the expenses of memory consumption.
     */
    unsigned int bComputedLeft[numLineLeft][numLineLeft]; //flag to show whether the ith pair of left image has already been computed.
    memset(bComputedLeft, 0, numLineLeft * numLineLeft * sizeof(unsigned int));
    double intersecRatioLeft[numLineLeft][numLineLeft]; //the ratio of intersection point and the line in the left pair
    double projRatioLeft[numLineLeft][numLineLeft];		//the point to line distance divided by the projected length of line in the left pair.

    unsigned int bComputedRight[numLineRight][numLineRight]; //flag to show whether the ith pair of right image has already been computed.
    memset(bComputedRight, 0, numLineRight * numLineRight * sizeof(unsigned int));
    double intersecRatioRight[numLineRight][numLineRight]; //the ratio of intersection point and the line in the right pair
    double projRatioRight[numLineRight][numLineRight];	 //the point to line distance divided by the projected length of line in the right pair.

    unsigned int idLeft1, idLeft2;						//the id of lines in the left pair
    unsigned int idRight1, idRight2;					//the id of lines in the right pair
    double relativeAngleLeft, relativeAngleRight;		//the relative angle of each line pair
    double gradientMagRatioLeft, gradientMagRatioRight; //the ratio of gradient magnitude of lines in each pair

    double iRatio1L, iRatio1R, iRatio2L, iRatio2R;
    double pRatio1L, pRatio1R, pRatio2L, pRatio2R;

    double relativeAngleDif, gradientMagRatioDif, iRatioDif, pRatioDif;

    double interSectionPointX, interSectionPointY;
    double a1, a2, b1, b2, c1, c2; //line1: a1 x + b1 y + c1 =0; line2: a2 x + b2 y + c2=0
    double a1b2_a2b1;			   //a1b2-a2b1
    double length1, length2, len;
    double disX, disY;
    double disS, disE;
    double similarity;


    for (unsigned int j = 0; j < dim; j++)
    { //column
        idLeft1 = nodesList_[j].leftLineID;
        idRight1 = nodesList_[j].rightLineID;
        for (unsigned int i = j + 1; i < dim; i++)
        { //row
            idLeft2 = nodesList_[i].leftLineID;
            idRight2 = nodesList_[i].rightLineID;
            if ((idLeft1 == idLeft2) || (idRight1 == idRight2))
            {
                continue; //not satisfy the one to one match condition
            }
            //first compute the relative angle between left pair and right pair.
            relativeAngleLeft = linesInLeft[idLeft1].angle - linesInLeft[idLeft2].angle;
            relativeAngleLeft = (relativeAngleLeft < M_PI) ? relativeAngleLeft : (relativeAngleLeft - TwoPI);
            relativeAngleLeft = (relativeAngleLeft > (-M_PI)) ? relativeAngleLeft : (relativeAngleLeft + TwoPI);
            relativeAngleRight = linesInRight[idRight1].angle - linesInRight[idRight2].angle;
            relativeAngleRight = (relativeAngleRight < M_PI) ? relativeAngleRight : (relativeAngleRight - TwoPI);
            relativeAngleRight = (relativeAngleRight > (-M_PI)) ? relativeAngleRight : (relativeAngleRight + TwoPI);
            relativeAngleDif = fabs(relativeAngleLeft - relativeAngleRight);
            if ((TwoPI - relativeAngleDif) > RelativeAngleDifferenceThreshold && relativeAngleDif > RelativeAngleDifferenceThreshold)
            {
                continue; //the relative angle difference is too large;
            }
            else if ((TwoPI - relativeAngleDif) < RelativeAngleDifferenceThreshold)
            {
                relativeAngleDif = TwoPI - relativeAngleDif;
            }

            //at last, check the intersect point ratio and point to line distance ratio
            //check whether the geometric information of pairs (idLeft1,idLeft2) and (idRight1,idRight2) have already been computed.
            if (!bComputedLeft[idLeft1][idLeft2])
            { //have not been computed yet

                a1 = linesInLeft[idLeft1].endPointY - linesInLeft[idLeft1].startPointY;					//disY
                b1 = linesInLeft[idLeft1].startPointX - linesInLeft[idLeft1].endPointX;					//-disX
                c1 = (0 - b1 * linesInLeft[idLeft1].startPointY) - a1 * linesInLeft[idLeft1].startPointX; //disX*sy - disY*sx
                length1 = linesInLeft[idLeft1].lineLength;

                a2 = linesInLeft[idLeft2].endPointY - linesInLeft[idLeft2].startPointY;					//disY
                b2 = linesInLeft[idLeft2].startPointX - linesInLeft[idLeft2].endPointX;					//-disX
                c2 = (0 - b2 * linesInLeft[idLeft2].startPointY) - a2 * linesInLeft[idLeft2].startPointX; //disX*sy - disY*sx
                length2 = linesInLeft[idLeft2].lineLength;

                a1b2_a2b1 = a1 * b2 - a2 * b1;
                if (fabs(a1b2_a2b1) < 0.001)
                { //two lines are almost parallel
                    iRatio1L = Inf;
                    iRatio2L = Inf;
                }
                else
                {
                    interSectionPointX = (c2 * b1 - c1 * b2) / a1b2_a2b1;
                    interSectionPointY = (c1 * a2 - c2 * a1) / a1b2_a2b1;
                    //r1 = (s1I*s1e1)/(|s1e1|*|s1e1|)
                    disX = interSectionPointX - linesInLeft[idLeft1].startPointX;
                    disY = interSectionPointY - linesInLeft[idLeft1].startPointY;
                    len = disY * a1 - disX * b1;    //交点到起点的距离与起点到终点的距离之比
                    iRatio1L = len / (length1 * length1);
                    //r2 = (s2I*s2e2)/(|s2e2|*|s2e2|)
                    disX = interSectionPointX - linesInLeft[idLeft2].startPointX;
                    disY = interSectionPointY - linesInLeft[idLeft2].startPointY;
                    len = disY * a2 - disX * b2;
                    iRatio2L = len / (length2 * length2);
                }
                intersecRatioLeft[idLeft1][idLeft2] = iRatio1L;
                intersecRatioLeft[idLeft2][idLeft1] = iRatio2L; //line order changed

                /*project the end points of line1 onto line2 and compute their distances to line2;
                 */
                disS = fabs(a2 * linesInLeft[idLeft1].startPointX + b2 * linesInLeft[idLeft1].startPointY + c2) / length2;
                //l1线段的起点到l2线段的距离与l2线段的长度之比
                disE = fabs(a2 * linesInLeft[idLeft1].endPointX + b2 * linesInLeft[idLeft1].endPointY + c2) / length2;
                //l1线段的终点到l2线段的距离与l2线段的长度之比
                pRatio1L = (disS + disE) / length1;
                projRatioLeft[idLeft1][idLeft2] = pRatio1L;

                /*project the end points of line2 onto line1 and compute their distances to line1;
                 */
                disS = fabs(a1 * linesInLeft[idLeft2].startPointX + b1 * linesInLeft[idLeft2].startPointY + c1) / length1;
                disE = fabs(a1 * linesInLeft[idLeft2].endPointX + b1 * linesInLeft[idLeft2].endPointY + c1) / length1;
                pRatio2L = (disS + disE) / length2;
                projRatioLeft[idLeft2][idLeft1] = pRatio2L;

                //mark them as computed
                bComputedLeft[idLeft1][idLeft2] = true;
                bComputedLeft[idLeft2][idLeft1] = true;
            }
            else
            { //read these information from matrix;
                iRatio1L = intersecRatioLeft[idLeft1][idLeft2];
                iRatio2L = intersecRatioLeft[idLeft2][idLeft1];
                pRatio1L = projRatioLeft[idLeft1][idLeft2];
                pRatio2L = projRatioLeft[idLeft2][idLeft1];
            }
            if (!bComputedRight[idRight1][idRight2])
            {
                //have not been computed yet
                a1 = linesInRight[idRight1].endPointY - linesInRight[idRight1].startPointY;					//disY
                b1 = linesInRight[idRight1].startPointX - linesInRight[idRight1].endPointX;					//-disX
                c1 = (0 - b1 * linesInRight[idRight1].startPointY) - a1 * linesInRight[idRight1].startPointX; //disX*sy - disY*sx
                length1 = linesInRight[idRight1].lineLength;

                a2 = linesInRight[idRight2].endPointY - linesInRight[idRight2].startPointY;					//disY
                b2 = linesInRight[idRight2].startPointX - linesInRight[idRight2].endPointX;					//-disX
                c2 = (0 - b2 * linesInRight[idRight2].startPointY) - a2 * linesInRight[idRight2].startPointX; //disX*sy - disY*sx
                length2 = linesInRight[idRight2].lineLength;

                a1b2_a2b1 = a1 * b2 - a2 * b1;
                if (fabs(a1b2_a2b1) < 0.001)
                { //two lines are almost parallel
                    iRatio1R = Inf;
                    iRatio2R = Inf;
                }
                else
                {
                    interSectionPointX = (c2 * b1 - c1 * b2) / a1b2_a2b1;
                    interSectionPointY = (c1 * a2 - c2 * a1) / a1b2_a2b1;
                    //r1 = (s1I*s1e1)/(|s1e1|*|s1e1|)
                    disX = interSectionPointX - linesInRight[idRight1].startPointX;
                    disY = interSectionPointY - linesInRight[idRight1].startPointY;
                    len = disY * a1 - disX * b1; //because b1=-disX
                    iRatio1R = len / (length1 * length1);
                    //r2 = (s2I*s2e2)/(|s2e2|*|s2e2|)
                    disX = interSectionPointX - linesInRight[idRight2].startPointX;
                    disY = interSectionPointY - linesInRight[idRight2].startPointY;
                    len = disY * a2 - disX * b2; //because b2=-disX
                    iRatio2R = len / (length2 * length2);
                }
                intersecRatioRight[idRight1][idRight2] = iRatio1R;
                intersecRatioRight[idRight2][idRight1] = iRatio2R; //line order changed
                /*project the end points of line1 onto line2 and compute their distances to line2;
                 */
                disS = fabs(a2 * linesInRight[idRight1].startPointX + b2 * linesInRight[idRight1].startPointY + c2) / length2;
                disE = fabs(a2 * linesInRight[idRight1].endPointX + b2 * linesInRight[idRight1].endPointY + c2) / length2;
                pRatio1R = (disS + disE) / length1;
                projRatioRight[idRight1][idRight2] = pRatio1R;

                /*project the end points of line2 onto line1 and compute their distances to line1;
                 */
                disS = fabs(a1 * linesInRight[idRight2].startPointX + b1 * linesInRight[idRight2].startPointY + c1) / length1;
                disE = fabs(a1 * linesInRight[idRight2].endPointX + b1 * linesInRight[idRight2].endPointY + c1) / length1;
                pRatio2R = (disS + disE) / length2;
                projRatioRight[idRight2][idRight1] = pRatio2R;

                //mark them as computed
                bComputedRight[idRight1][idRight2] = true;
                bComputedRight[idRight2][idRight1] = true;
            }
            else
            { //read these information from matrix;
                iRatio1R = intersecRatioRight[idRight1][idRight2];
                iRatio2R = intersecRatioRight[idRight2][idRight1];
                pRatio1R = projRatioRight[idRight1][idRight2];
                pRatio2R = projRatioRight[idRight2][idRight1];
            }

            pRatioDif = MIN(fabs(pRatio1L - pRatio1R), fabs(pRatio2L - pRatio2R));
            //cout<<"pRatioDif="<<pRatioDif<<endl;
            if (pRatioDif > ProjectionRationDifThreshold)
            {
                continue; //the projection length ratio difference is too large;
            }
            if ((iRatio1L == Inf) || (iRatio2L == Inf) || (iRatio1R == Inf) || (iRatio2R == Inf))
            {
                //don't consider the intersection length ratio
                similarity = 4 - desDisMat[idLeft1][idRight1] / BinaryDescriptorDifThreshold - desDisMat[idLeft2][idRight2] / BinaryDescriptorDifThreshold - pRatioDif / ProjectionRationDifThreshold - relativeAngleDif / RelativeAngleDifferenceThreshold;
                adjacenceVec[(2 * dim - j - 1) * j / 2 + i] = similarity;
                nnz++;
                //				testMat[i][j] = similarity;
                //				testMat[j][i] = similarity;
            }
            else
            {
                iRatioDif = MIN(fabs(iRatio1L - iRatio1R), fabs(iRatio2L - iRatio2R));
                if (iRatioDif > IntersectionRationDifThreshold)
                {
                    continue; //the intersection length ratio difference is too large;
                }
                //now compute the similarity score between two line pairs.
                similarity = 5 - desDisMat[idLeft1][idRight1] / BinaryDescriptorDifThreshold - desDisMat[idLeft2][idRight2] / BinaryDescriptorDifThreshold - iRatioDif / IntersectionRationDifThreshold - pRatioDif / ProjectionRationDifThreshold - relativeAngleDif / RelativeAngleDifferenceThreshold;
                adjacenceVec[(2 * dim - j - 1) * j / 2 + i] = similarity;
                nnz++;
                //				testMat[i][j] = similarity;
                //				testMat[j][i] = similarity;
            }
        }
    }
    //cout<<"nnz="<<nnz<<endl;
    // pointer to an array that stores the nonzero elements of Adjacency matrix.
    double *adjacenceMat = new double[nnz];
    // pointer to an array that stores the row indices of the non-zeros in adjacenceMat.
    int *irow = new int[nnz];
    // pointer to an array of pointers to the beginning of each column of adjacenceMat.
    int *pcol = new int[dim + 1];
    int idOfNNZ = 0; //the order of none zero element
    pcol[0] = 0;
    unsigned int tempValue;
    for (unsigned int j = 0; j < dim; j++)
    { //column
        for (unsigned int i = j; i < dim; i++)
        { //row
            tempValue = (2 * dim - j - 1) * j / 2 + i;
            if (adjacenceVec[tempValue] != 0)
            {
                adjacenceMat[idOfNNZ] = adjacenceVec[tempValue];
                irow[idOfNNZ] = i;
                idOfNNZ++;
            }
        }
        pcol[j + 1] = idOfNNZ;
    }
    /*Third step, solve the principal eigenvector of the adjacency matrix using Arpack lib.
     */
    ARluSymMatrix<double> arMatrix(dim, nnz, adjacenceMat, irow, pcol);
    ARluSymStdEig<double> dprob(2, arMatrix, "LM"); // Defining what we need: the first eigenvector of arMatrix with largest magnitude.
    // Finding eigenvalues and eigenvectors.
    dprob.FindEigenvectors();
    cout << "Number of 'converged' eigenvalues  : " << dprob.ConvergedEigenvalues() << endl;

    eigenMap_.clear();

    double meanEigenVec = 0;
    if (dprob.EigenvectorsFound())
    {
        double value;
        for (unsigned int j = 0; j < dim; j++)
        {
            value = fabs(dprob.Eigenvector(1, j));
            meanEigenVec += value;
            eigenMap_.insert(std::make_pair(value, j));
        }
    }
    minOfEigenVec_ = WeightOfMeanEigenVec * meanEigenVec / dim;
    delete[] adjacenceMat;
    delete[] irow;
    delete[] pcol;
}

void PairwiseLineMatching::MatchingResultFromPrincipalEigenvector_(ScaleLines &linesInLeft, ScaleLines &linesInRight, std::vector<unsigned int> &matchResult)
{
	double TwoPI = 2 * M_PI;
	std::vector<unsigned int> matchRet1;
	std::vector<unsigned int> matchRet2;
	double matchScore1 = 0;
	double matchScore2 = 0;
	EigenMAP mapCopy = eigenMap_;
	unsigned int dim = nodesList_.size();
	EigenMAP::iterator iter;
	unsigned int id, idLeft2, idRight2;
	double sideValueL, sideValueR;
	double pointX, pointY;
	double relativeAngleLeft, relativeAngleRight; //the relative angle of each line pair
	double relativeAngleDif;

	//store eigenMap for debug
	std::fstream resMap;
	ostringstream fileNameMap;
	fileNameMap << "eigenVec.txt";
	resMap.open(fileNameMap.str().c_str(), std::ios::out);

	double mat[linesInLeft.size()][linesInRight.size()];
	memset(mat, 0, linesInLeft.size() * linesInRight.size());
	for (iter = eigenMap_.begin(); iter != eigenMap_.end(); iter++)
	{
		id = iter->second;
		resMap << nodesList_[id].leftLineID << "    " << nodesList_[id].rightLineID << "   " << iter->first << endl;
		mat[nodesList_[id].leftLineID][nodesList_[id].rightLineID] = iter->first;
	}
	//mat.Save("eigenMap.txt");
	// matSave("eigenMap.txt");
	resMap.flush();
	resMap.close();

	/*first try, start from the top element in eigenmap */
	while (1)
	{
		iter = eigenMap_.begin();
		//if the top element in the map has small value, then there is no need to continue find more matching line pairs;
		if (iter->first < minOfEigenVec_)
		{
			break;
		}
		id = iter->second;
		unsigned int idLeft1 = nodesList_[id].leftLineID;
		unsigned int idRight1 = nodesList_[id].rightLineID;
		matchRet1.push_back(idLeft1);
		matchRet1.push_back(idRight1);
		matchScore1 += iter->first;
		eigenMap_.erase(iter++);
		//remove all potential assignments in conflict with top matched line pair
		double xe_xsLeft = linesInLeft[idLeft1][0].endPointX - linesInLeft[idLeft1][0].startPointX;
		double ye_ysLeft = linesInLeft[idLeft1][0].endPointY - linesInLeft[idLeft1][0].startPointY;
		double xe_xsRight = linesInRight[idRight1][0].endPointX - linesInRight[idRight1][0].startPointX;
		double ye_ysRight = linesInRight[idRight1][0].endPointY - linesInRight[idRight1][0].startPointY;
		double coefLeft = sqrt(xe_xsLeft * xe_xsLeft + ye_ysLeft * ye_ysLeft);
		double coefRight = sqrt(xe_xsRight * xe_xsRight + ye_ysRight * ye_ysRight);
		for (; iter->first >= minOfEigenVec_;)
		{
			id = iter->second;
			idLeft2 = nodesList_[id].leftLineID;
			idRight2 = nodesList_[id].rightLineID;
			//check one to one match condition
			if ((idLeft1 == idLeft2) || (idRight1 == idRight2))
			{
				eigenMap_.erase(iter++);
				continue; //not satisfy the one to one match condition
			}
			//check sidedness constraint, the middle point of line2 should lie on the same side of line1.
			//sideValue = (y-ys)*(xe-xs)-(x-xs)*(ye-ys);
			pointX = 0.5 * (linesInLeft[idLeft2][0].startPointX + linesInLeft[idLeft2][0].endPointX);
			pointY = 0.5 * (linesInLeft[idLeft2][0].startPointY + linesInLeft[idLeft2][0].endPointY);
			sideValueL = (pointY - linesInLeft[idLeft1][0].startPointY) * xe_xsLeft - (pointX - linesInLeft[idLeft1][0].startPointX) * ye_ysLeft;
			sideValueL = sideValueL / coefLeft;

			pointX = 0.5 * (linesInRight[idRight2][0].startPointX + linesInRight[idRight2][0].endPointX);
			pointY = 0.5 * (linesInRight[idRight2][0].startPointY + linesInRight[idRight2][0].endPointY);
			sideValueR = (pointY - linesInRight[idRight1][0].startPointY) * xe_xsRight - (pointX - linesInRight[idRight1][0].startPointX) * ye_ysRight;
			sideValueR = sideValueR / coefRight;
			if (sideValueL * sideValueR < 0 && fabs(sideValueL) > 5 && fabs(sideValueR) > 5)
			{ //have the different sign, conflict happens.
				eigenMap_.erase(iter++);
				continue;
			}
			//check relative angle difference
			relativeAngleLeft = linesInLeft[idLeft1][0].direction - linesInLeft[idLeft2][0].direction;
			relativeAngleLeft = (relativeAngleLeft < M_PI) ? relativeAngleLeft : (relativeAngleLeft - TwoPI);
			relativeAngleLeft = (relativeAngleLeft > (-M_PI)) ? relativeAngleLeft : (relativeAngleLeft + TwoPI);
			relativeAngleRight = linesInRight[idRight1][0].direction - linesInRight[idRight2][0].direction;
			relativeAngleRight = (relativeAngleRight < M_PI) ? relativeAngleRight : (relativeAngleRight - TwoPI);
			relativeAngleRight = (relativeAngleRight > (-M_PI)) ? relativeAngleRight : (relativeAngleRight + TwoPI);
			relativeAngleDif = fabs(relativeAngleLeft - relativeAngleRight);
			if ((TwoPI - relativeAngleDif) > RelativeAngleDifferenceThreshold && relativeAngleDif > RelativeAngleDifferenceThreshold)
			{
				eigenMap_.erase(iter++);
				continue; //the relative angle difference is too large;
			}
			iter++;
		}
	} //end while(stillLoop)
	matchResult = matchRet1;
	cout << "matchRet1.size=" << matchRet1.size() << ", minOfEigenVec_= " << minOfEigenVec_ << endl;
}


void PairwiseLineMatching::BinaryMatchingResultFromPrincipalEigenvector_(vector<KeyLine> &linesInLeft,vector<KeyLine> &linesInRight, Mat &left_ldesc,
                                                                         Mat &right_ldesc,std::vector<unsigned int> &matchResult)
{
    double TwoPI = 2 * M_PI;
    std::vector<unsigned int> matchRet1;
    std::vector<unsigned int> matchRet2;
    double matchScore1 = 0;
    double matchScore2 = 0;
    EigenMAP mapCopy = eigenMap_;
    unsigned int dim = nodesList_.size();
    EigenMAP::iterator iter;
    unsigned int id, idLeft2, idRight2;
    double sideValueL, sideValueR;
    double pointX, pointY;
    double relativeAngleLeft, relativeAngleRight; //the relative angle of each line pair
    double relativeAngleDif;

    //store eigenMap for debug
    std::fstream resMap;
    ostringstream fileNameMap;
    fileNameMap << "eigenVec.txt";
    resMap.open(fileNameMap.str().c_str(), std::ios::out);

    double mat[linesInLeft.size()][linesInRight.size()];
    memset(mat, 0, linesInLeft.size() * linesInRight.size());
    for (iter = eigenMap_.begin(); iter != eigenMap_.end(); iter++)
    {
        id = iter->second;
        resMap << nodesList_[id].leftLineID << "    " << nodesList_[id].rightLineID << "   " << iter->first << endl;
        mat[nodesList_[id].leftLineID][nodesList_[id].rightLineID] = iter->first;
    }
    //mat.Save("eigenMap.txt");
    // matSave("eigenMap.txt");
    resMap.flush();
    resMap.close();

    /*first try, start from the top element in eigenmap */
    while (1)
    {
        iter = eigenMap_.begin();
        //if the top element in the map has small value, then there is no need to continue find more matching line pairs;
        if (iter->first < minOfEigenVec_)
        {
            break;
        }
        id = iter->second;
        unsigned int idLeft1 = nodesList_[id].leftLineID;
        unsigned int idRight1 = nodesList_[id].rightLineID;
        matchRet1.push_back(idLeft1);
        matchRet1.push_back(idRight1);
        matchScore1 += iter->first;
        eigenMap_.erase(iter++);
        //remove all potential assignments in conflict with top matched line pair
        double xe_xsLeft = linesInLeft[idLeft1].endPointX - linesInLeft[idLeft1].startPointX;
        double ye_ysLeft = linesInLeft[idLeft1].endPointY - linesInLeft[idLeft1].startPointY;
        double xe_xsRight = linesInRight[idRight1].endPointX - linesInRight[idRight1].startPointX;
        double ye_ysRight = linesInRight[idRight1].endPointY - linesInRight[idRight1].startPointY;
        double coefLeft = sqrt(xe_xsLeft * xe_xsLeft + ye_ysLeft * ye_ysLeft);
        double coefRight = sqrt(xe_xsRight * xe_xsRight + ye_ysRight * ye_ysRight);
        for (; iter->first >= minOfEigenVec_;)
        {
            id = iter->second;
            idLeft2 = nodesList_[id].leftLineID;
            idRight2 = nodesList_[id].rightLineID;
            //check one to one match condition
            if ((idLeft1 == idLeft2) || (idRight1 == idRight2))
            {
                eigenMap_.erase(iter++);
                continue; //not satisfy the one to one match condition
            }
            //check sidedness constraint, the middle point of line2 should lie on the same side of line1.
            //sideValue = (y-ys)*(xe-xs)-(x-xs)*(ye-ys);
            pointX = 0.5 * (linesInLeft[idLeft2].startPointX + linesInLeft[idLeft2].endPointX);
            pointY = 0.5 * (linesInLeft[idLeft2].startPointY + linesInLeft[idLeft2].endPointY);
            sideValueL = (pointY - linesInLeft[idLeft1].startPointY) * xe_xsLeft - (pointX - linesInLeft[idLeft1].startPointX) * ye_ysLeft;
            sideValueL = sideValueL / coefLeft;

            pointX = 0.5 * (linesInRight[idRight2].startPointX + linesInRight[idRight2].endPointX);
            pointY = 0.5 * (linesInRight[idRight2].startPointY + linesInRight[idRight2].endPointY);
            sideValueR = (pointY - linesInRight[idRight1].startPointY) * xe_xsRight - (pointX - linesInRight[idRight1].startPointX) * ye_ysRight;
            sideValueR = sideValueR / coefRight;
            if (sideValueL * sideValueR < 0 && fabs(sideValueL) > 5 && fabs(sideValueR) > 5)
            { //have the different sign, conflict happens.
                eigenMap_.erase(iter++);
                continue;
            }
            //check relative angle difference
            relativeAngleLeft = linesInLeft[idLeft1].angle - linesInLeft[idLeft2].angle;
            relativeAngleLeft = (relativeAngleLeft < M_PI) ? relativeAngleLeft : (relativeAngleLeft - TwoPI);
            relativeAngleLeft = (relativeAngleLeft > (-M_PI)) ? relativeAngleLeft : (relativeAngleLeft + TwoPI);
            relativeAngleRight = linesInRight[idRight1].angle - linesInRight[idRight2].angle;
            relativeAngleRight = (relativeAngleRight < M_PI) ? relativeAngleRight : (relativeAngleRight - TwoPI);
            relativeAngleRight = (relativeAngleRight > (-M_PI)) ? relativeAngleRight : (relativeAngleRight + TwoPI);
            relativeAngleDif = fabs(relativeAngleLeft - relativeAngleRight);
            if ((TwoPI - relativeAngleDif) > RelativeAngleDifferenceThreshold && relativeAngleDif > RelativeAngleDifferenceThreshold)
            {
                eigenMap_.erase(iter++);
                continue; //the relative angle difference is too large;
            }
            iter++;
        }
    } //end while(stillLoop)
    matchResult = matchRet1;
    cout << "matchRet1.size=" << matchRet1.size() << ", minOfEigenVec_= " << minOfEigenVec_ << endl;
}